using System;
using Microsoft.SPOT;
using GHIElectronics.NETMF.Hardware;
using GHIElectronics.NETMF.FEZ;
using System.Threading;
using NetMf.CommonExtensions;
using System.Text;
using System.IO;

namespace AstroX
{
    public class AccelerometerController
    {
        #region Acceleration Inputs
        AnalogIn x18G = new AnalogIn(AstoXPinMap.X18G);
        AnalogIn y18G = new AnalogIn(AstoXPinMap.Y18G);
        AnalogIn x5G = new AnalogIn(AstoXPinMap.X5G);
        AnalogIn y5G = new AnalogIn(AstoXPinMap.Y5G);
        AnalogIn z250G = new AnalogIn(AstoXPinMap.Z250G);
        AnalogIn z18G = new AnalogIn(AstoXPinMap.Z18G);
        #endregion

        private string _filepath = "data.bin";
        FileStream fs;
        public AccelerometerController()
        {
            _filepath = Path.Combine(Program.rootDirectory, _filepath);
            fs = new FileStream(_filepath, FileMode.OpenOrCreate, FileAccess.Write);
            FlightControl.FlightPhaseChanged += new FlightPhaseChangeHandler(FlightControl_FlightPhaseChanged);

        }

        void FlightControl_FlightPhaseChanged(FlightPhases newphase)
        {
            if (newphase == FlightPhases.StandingBy)
            {
                Program.Print("Spawning sensor thread");
                Thread t = new Thread(() => HighSpeedRead());
                t.Start();
            }
        }

        public void HighSpeedRead()
        {
            Program.Print("Starting data sampling");
            int val = 0;
            long ticks = 0L;
            byte[] data = new byte[20];
            while (FlightControl.CurrentPhase!=FlightPhases.Shutdown)
            {
            
                
                val = x18G.Read();
                data[0] = (byte)(val >> 8); data[1] = (byte)(val);

                val = y18G.Read();
                data[2] = (byte)(val >> 8); data[3] = (byte)(val);

                val = x5G.Read();
                data[4] = (byte)(val >> 8); data[5] = (byte)(val);

                val = y5G.Read();
                data[6] = (byte)(val >> 8); data[7] = (byte)(val);

                val = z250G.Read();
                data[8] = (byte)(val >> 8); data[9] = (byte)(val);

                val = z18G.Read();
                data[10] = (byte)(val >> 8); data[11] = (byte)(val);

                
                ticks = DateTime.Now.Ticks;
                //var l=0L;
                data[12] = (byte)ticks;
                data[13] = (byte)(ticks >> 8);
                data[14] = (byte)(ticks >> 16);
                data[15] = (byte)(ticks >> 24);
                data[16] = (byte)(ticks >> 32);
                data[17] = (byte)(ticks >> 40);
                data[18] = (byte)(ticks >> 48);
                data[19] = (byte)(ticks >> 56);

                fs.Write(data, 0, 20);
                
            }
            Program.Print("Data sampling complete");
            fs.Flush();
            fs.Close();
        
            
        }
    }
}
